/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.templates.RobotMap;

/**
 *
 * @author matt
 */
public class RobotState {

    DigitalInput robotID = new DigitalInput(RobotMap.kRobotIDPort);

        // Robot Heading Enumeration
    public static class RobotID {

        public final int value;
        // Robot Heading enumeration constants
        protected static final int m_kKitty = 0;
        protected static final int m_kRook = 1;
        public static final RobotID kKitty = new RobotID(m_kKitty);
        public static final RobotID kRook = new RobotID(m_kRook);

        private RobotID(int robotId) {
            this.value = robotId;
        }
    }

    
    // Robot Heading Enumeration
    public static class RobotDirection {

        public final int value;
        // Robot Heading enumeration constants
        protected static final int m_kForward = 0;
        protected static final int m_kLeft = 1;
        protected static final int m_kRight = 2;
        protected static final int m_kReverse = 3;
        public static final RobotDirection kForward = new RobotDirection(m_kForward);
        public static final RobotDirection kLeft = new RobotDirection(m_kLeft);
        public static final RobotDirection kRight = new RobotDirection(m_kRight);
        public static final RobotDirection kReverse = new RobotDirection(m_kReverse);

        private RobotDirection(int robotHeading) {
            this.value = robotHeading;
        }
    }
    // Singleton Stuff
    private final static RobotState robotStateSingleton = new RobotState();

    public static RobotState getInstance() {
        return robotStateSingleton;
    }

    private RobotState() {
    }
    // Robot State information starts here
    private RobotDirection m_robotDirection = RobotDirection.kForward;

    public RobotID getRobot() {
        // Kitty has a jumper on DIO 14
        //return !robotID.get();
        return RobotID.kRook;
    }

    public void setDirection(RobotDirection direction) {
        m_robotDirection = direction;
    }

    public RobotDirection getDirection() {
        return m_robotDirection;
    }
    private static int m_numRecoverableSubsystems = 0;
    Recoverable[] recoverableSubsystem = {null, null, null, null, null, null, null, null, null, null};

    /*
     * Subsystem recovery stuff below here.
     *
     *
     *
     */
    // Thread class to periodically check to see if there are any 
    // recovery actions to take
    private class RecoveryThread implements Runnable {

        public void run() {
            Timer.delay(0.2);
            recover(false);
        }
    }

    public void registerForRecovery(Recoverable subsystem) {
        if (null != subsystem) {
            recoverableSubsystem[m_numRecoverableSubsystems] = subsystem;
            System.out.println("Recovery - registering subsystem # " + m_numRecoverableSubsystems);
            m_numRecoverableSubsystems++;
        }
        if (m_numRecoverableSubsystems == 1) {
            System.out.println("Recovery - Starting Recovery Thread");
            // start the recovery check thread if there is at least one recoverable subsystem
            (new Thread(new RecoveryThread())).start();
        }
    }

    public void recover(boolean override) {
        if (m_numRecoverableSubsystems > 0) {
            for (int idx = 0; idx < m_numRecoverableSubsystems; idx++) {
                if (null != recoverableSubsystem[idx]) {
                    recoverableSubsystem[idx].recover(override);
                }
            }
        }
    }
}
